#ifndef SERVO_H
#define SERVO_H

#include <stdio.h>
#include <wiringPi.h>

#define PIN_KEY1 26
#define PIN_KEY2 23
#define PIN_LED 7
#define PIN_SERVO 28

#define PERIOD 20000
#define ANGLE_ZERO 1500
#define ANGLE_MIN 500
#define ANGLE_MAX 2500

static unsigned int angle = ANGLE_ZERO;
static int quit = 0;
static int running = 0;

void wiringPiStart(void)
{
	wiringPiSetup();
	
	pinMode(PIN_SERVO, OUTPUT);
	digitalWrite(PIN_SERVO,0);
}

void Servo_start(void)
{
    quit=0;
    running = 1;
    while(!quit){
		digitalWrite(PIN_SERVO,1);
		delayMicroseconds(angle);
		digitalWrite(PIN_SERVO,0);
		delayMicroseconds(PERIOD-angle);
	}
    return;
}
void angle_cw(void)
{
    if(running)
	if(angle<ANGLE_MAX)
	angle += 100;
}
void angle_ccw(void)
{
    if(running)
	if(angle>ANGLE_MIN)
	angle -= 100;
}

void Servo_Stop(void)
{
    quit=1;
    running=0;
    angle = ANGLE_ZERO;
    return;
}

#endif
